/*
 * CDNET Motor Test
 * Testing CDNET protocol with motor control
 * 
 * This example demonstrates the integration of CDNET protocol
 * with stepper motor control functionality
 */

#include "SimpleCDNET.h"
#include "SimpleCDMOTOR.h"

SimpleCDNET cdnet;
SimpleCDMOTOR motor;

// Motor configuration
motor_config_t motor_config = {
    .enable_pin = 8,
    .dir_pin = 9,
    .step_pin = 10,
    .ms1_pin = 11,
    .ms2_pin = 12,
    .ms3_pin = 13,
    .home_pin = 2,
    .limit_pin = 3,
    .steps_per_rev = 200,
    .max_speed = 800,
    .max_accel = 4000,
    .current_limit = 1000,
    .kp = 1.5,
    .ki = 0.3,
    .kd = 0.1
};

// CDNET packet structure for motor commands
struct MotorPacket {
    uint8_t command;
    int32_t position;
    int32_t velocity;
    uint16_t flags;
} __attribute__((packed));

void setup() {
    Serial.begin(115200);
    while (!Serial);
    
    Serial.println("CDNET Motor Test");
    Serial.println("================");
    
    // Initialize CDNET
    cdnet.begin(&Serial1, 0x01);  // Address 0x01
    
    // Initialize motor
    motor.begin(&Serial1, 0x01, 0x1002);
    motor.configure(motor_config);
    
    Serial.println("Initialized CDNET + Motor");
    Serial.println("Waiting for packets...");
}

void loop() {
    // Update both systems
    cdnet.update();
    motor.update();
    
    // Handle incoming CDNET packets
    if (cdnet.available()) {
        uint8_t buffer[64];
        uint8_t length = sizeof(buffer);
        uint16_t src_addr;
        uint8_t port;
        
        if (cdnet.read(buffer, &length, &src_addr, &port)) {
            handleMotorPacket(buffer, length, src_addr, port);
        }
    }
    
    // Periodically broadcast status
    static uint32_t last_status = 0;
    if (millis() - last_status > 1000) {
        broadcastStatus();
        last_status = millis();
    }
}

void handleMotorPacket(uint8_t* data, uint8_t len, uint16_t src_addr, uint8_t port) {
    if (len < sizeof(MotorPacket)) {
        Serial.println("Invalid packet size");
        return;
    }
    
    MotorPacket* pkt = (MotorPacket*)data;
    
    Serial.print("Received command: ");
    Serial.print(pkt->command);
    Serial.print(" from 0x");
    Serial.println(src_addr, HEX);
    
    switch (pkt->command) {
        case 0x01:  // Move to position
            motor.setPosition(pkt->position);
            Serial.print("Moving to: ");
            Serial.println(pkt->position);
            break;
            
        case 0x02:  // Set velocity
            motor.setVelocity(pkt->velocity);
            Serial.print("Setting velocity: ");
            Serial.println(pkt->velocity);
            break;
            
        case 0x03:  // Home
            motor.home();
            Serial.println("Homing");
            break;
            
        case 0x04:  // Enable
            motor.enable();
            Serial.println("Enabling motor");
            break;
            
        case 0x05:  // Disable
            motor.disable();
            Serial.println("Disabling motor");
            break;
            
        case 0x06:  // Stop
            motor.stop();
            Serial.println("Stopping motor");
            break;
            
        case 0x07:  // Emergency stop
            motor.stop();
            motor.disable();
            Serial.println("Emergency stop");
            break;
            
        default:
            Serial.println("Unknown command");
            break;
    }
}

void broadcastStatus() {
    struct StatusPacket {
        uint8_t type = 0x01;
        int32_t position;
        int32_t target;
        uint8_t state;
        uint8_t flags;
    } __attribute__((packed));
    
    StatusPacket status;
    status.position = motor.getPosition();
    status.target = motor.getTargetPosition();
    status.state = motor.getState();
    status.flags = (motor.isHomed() ? 0x01 : 0x00) | 
                   (motor.isMoving() ? 0x02 : 0x00) |
                   (motor.isEnabled() ? 0x04 : 0x00);
    
    cdnet.send((uint8_t*)&status, sizeof(status), 0xFFFF, 0x1002);
    
    Serial.print("Broadcasting status: pos=");
    Serial.print(status.position);
    Serial.print(" target=");
    Serial.print(status.target);
    Serial.print(" state=");
    Serial.println(status.state);
}